#!/usr/bin/python3
from xml.dom import HierarchyRequestErr
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from ackermann_msgs.msg import AckermannDriveStamped
import time
import cv2
import os
import sys
import glob
import numpy as np
import math
from matplotlib import pyplot as plt
 
def cv_show(img):
    cv2.imshow('show', img)
    # if cv2.waitKey(0) & 0xff == ord('s'):
    #     cv2.imwrite('~/temp.png')


def camera_callback(img):
    # print("Callback!")
    global bridge
    bridge = CvBridge()
    src = img.data
    cv_img = bridge.imgmsg_to_cv2(img, "bgr8")
    detectStopSign(cv_img)
    cv2.waitKey(3)


def detectStopSign(img):
    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    img_thresh = cv2.inRange(img_hsv, np.array([0, 100, 0]), np.array([10, 255, 255]))
    kernel = np.ones((3, 3))
    img_thresh = cv2.morphologyEx(img_thresh, cv2.MORPH_CLOSE, kernel)
    # cv2.imshow('red', img_thresh)
    contours, hierarchy = cv2.findContours(img_thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    contour_circle = []
    for c in contours:
        contour_poly = cv2.approxPolyDP(c, 0.01* cv2.arcLength(c, True), True)
        cv2.imshow('contour', cv2.drawContours(img.copy(), [contour_poly], -1, (0, 255, 0), 1))
        cv2.waitKey(0)
        if len(contour_poly) > 10:
            # print(cv2.arcLength(c, True))
            if 250 < cv2.arcLength(c, True) < 300:
                contour_circle.append(c)
                print("Stop sign nearby detected!")
            break
    cv2.imshow('contour', cv2.drawContours(img.copy(), contour_circle, -1, (0, 255, 0), 1))


if __name__ == '__main__':
    try:
        print("exec!")
        rospy.init_node('stop_sign_detect', anonymous=True)
        rospy.Subscriber("/deepracer1/camera/zed_left/image_rect_color_left", Image, camera_callback)
        rospy.spin()
    except rospy.ROSInterruptException:
        pass